/* Copyright 2001,2002,2003 NAH6
 * All Rights Reserved
 *
 * Parts Copyright DoD, Parts Copyright Starium
 *
 */
#include <stdio.h>
#include "main.h"
#include "interp.h"

STATIC void InterpolateSubFrame(
fxpt_16 cur_lsf[ORDER],
fxpt_16 prev_lsf[ORDER],
fxpt_16 lsfint[NUM_SF][ORDER],
int     sf);

STATIC void AnalCheckForMonotonicity(
fxpt_16 cur_lsfint[NUM_SF][ORDER],
fxpt_16 prev_lsfint[ORDER],
int     sf
);

STATIC void SynthCheckForMonotonicity(
fxpt_16 cur_lsf[ORDER],
fxpt_16 prev_lsf[ORDER]);

/**************************************************************************
*                                                                         *
* ROUTINE
*               Interpolate
*
* FUNCTION
*               Interpolation of LSFs
* SYNOPSIS
*               Interpolate(lsf, lsfint, where, frame_num)
*
*   formal
*
*                       data    I/O
*       name            type    type    function
*       -------------------------------------------------------------------
*       lsf             fxpt_16  i      Frame of LSFs
*       lsfint          fxpt_16  o      Frame of interpolated LSFs
*       where           int      i      analysis/synthesis flag
*       frame_num       int      i      Current frame number
*
**************************************************************************
*
* CALLED BY
*
*       Analysis Synthesis
*
**************************************************************************/
void Interpolate(
fxpt_16 lsf[ORDER],                     /* 0.15 format */
fxpt_16 lsfint[NUM_SF][ORDER],          /* 0.15 format */
int     where
)
{
        int             i;
        static fxpt_16  prev_ana_sf_lsf[ORDER];
        /* Converted to 0.15 format */
        static fxpt_16  prev_ana_ff_lsf[ORDER]= {
                  983,  1638,  2949,  4260,  6226,
                 7537,  9503, 10813, 12780, 14418
        };
        static fxpt_16  prev_syn_lsf[ORDER]= {
                  983,  1638,  2949,  4260,  6226,
                 7537,  9503, 10813, 12780, 14418
        };

FXPT_PUSHSTATE("Interpolate", -1.0, -1.0);
        /*  Perform interpolation depending on whether this is
         *  analysis or synthesis
         */
        switch (where) {
        /*  Analysis interpolation */
        /*-------------------------------------------------------------------*/
        case ANALYSIS:
                for (i=0; i<NUM_SF; i++) {

                        /*  Interpolate for each subframe into
                         *  interpolation matrix
                         */
                        InterpolateSubFrame(lsf, prev_ana_ff_lsf, lsfint, i);

                        /*  Check for Nonmonotonic LSFs */
                        AnalCheckForMonotonicity(lsfint, prev_ana_sf_lsf, i);
                }

                /*  Update values for next pass */
                for (i=0; i<ORDER; i++) {
                        /*  Update previous subframe LSFs */
                        prev_ana_sf_lsf[i] = lsfint[NUM_SF-1][i];
                        /*  Update previous full frame LSFs */
                        prev_ana_ff_lsf[i] = lsf[i];
                }

                break;
        /*-------------------------------------------------------------------*/
        case SYNTHESIS:

                /*  Check for Nonmonotonic LSFs */
                SynthCheckForMonotonicity(lsf, prev_syn_lsf);

                /*  Interpolate LSFs for each subframe into interpolation
                 *  matrix
                 */
                for (i=0; i<NUM_SF; i++) {
                        InterpolateSubFrame(lsf, prev_syn_lsf, lsfint, i);
                }

                /*  Update LSF history */
                for (i=0; i<ORDER; i++) {
                        prev_syn_lsf[i] = lsf[i];
                }

                break;
        /*-------------------------------------------------------------------*/
        }
FXPT_POPSTATE();
}

/**************************************************************************
*                                                                         *
* ROUTINE
*               InterpolateSubFrame
*
* FUNCTION
*               Interpolation one LSF subframe
* SYNOPSIS
*               InterpolateSubFrame(cur_lsf, prev_lsf, lsfint, sf)
*
*   formal
*
*                       data    I/O
*       name            type    type    function
*       -------------------------------------------------------------------
*       cur_lsf         fxpt_16  i      Current Frame of LSFs
*       prev_lsf        fxpt_16  i      Previous Frame of LSFs
*       lsfint          fxpt_16  o      Frame of interpolated LSFs
*       sf              int      i      Current subframe number
*
**************************************************************************/

/* Converted to 0.15 format (multiply by 2^15) */
fxpt_16 win[2][4] = {   { 28672, 20480, 12288, 4096 },
                        { 4096,  12288, 20480, 28672  }  };

void InterpolateSubFrame(
fxpt_16 cur_lsf[ORDER],
fxpt_16 prev_lsf[ORDER],
fxpt_16 lsfint[NUM_SF][ORDER],
int     sf)
{
        int             i;

        /*  Interpolate for this subframe */
        for (i=0; i<ORDER; i++) {
                lsfint[sf][i] = fxpt_add16(
                    fxpt_mult16_round(win[0][sf], prev_lsf[i], 15),
                    fxpt_mult16_round(win[1][sf], cur_lsf[i], 15));
        }
}

/**************************************************************************
*                                                                         *
* ROUTINE
*               AnalCheckForMonotonicity
*
* FUNCTION
*               Check for nonmonotonic LSFs within interpolated matrix
* SYNOPSIS
*               AnalCheckForMonotonicity(cur_lsfint, prev_lsfint, sf, frame_num)
*
*   formal
*
*                       data    I/O
*       name            type    type    function
*       -------------------------------------------------------------------
*       cur_lsfint      fxpt_16 i/o     Current frame of interpolated LSFs
*       prev_lsfint     fxpt_16  o      Previous frame of interpolated LSFs
*       sf              int      i      Current sub frame number
*       frame_num       int      i      Current frame number
*
**************************************************************************/

void AnalCheckForMonotonicity(
fxpt_16 cur_lsfint[NUM_SF][ORDER],
fxpt_16 prev_lsfint[ORDER],
int     sf
)
{
        int     nonmono=FALSE;
        int     i;
        fxpt_16 temp;

        /*  Check for monotonically increasing LSFs, swap crossed LSFs */
        for (i=1; i<ORDER; i++) {
                if (cur_lsfint[sf][i] <= cur_lsfint[sf][i-1]) {
                        CELP_PRINTF(("Analysis Interpolation:\n\tSwapping nonmono LSFs in frame\n"));
                        temp = cur_lsfint[sf][i];
                        cur_lsfint[sf][i] = cur_lsfint[sf][i-1];
                        cur_lsfint[sf][i-1] = temp;
                }
        }

        /*  Recheck for monotonically increasing LSFs & substitute old LSFs */
        for (i=1; i<ORDER; i++) {
                if (cur_lsfint[sf][i] <= cur_lsfint[sf][i-1])
                nonmono = TRUE;
        }
        if (nonmono) {
                CELP_PRINTF(("Analysis Interpolation:\n\tResetting interpolated LSF in frame\n"));
                for (i=0; i<ORDER; i++) {
                        if (i==0)
                                cur_lsfint[sf][i] = prev_lsfint[i];
                        else
                                cur_lsfint[sf][i] = cur_lsfint[sf-1][i];
                }
        }
}

/**************************************************************************
*                                                                         *
* ROUTINE
*               SynthCheckForMonotonicity
*
* FUNCTION
*               Check for nonmonotonic LSFs
* SYNOPSIS
*               SynthCheckForMonotonicity()
*
*   formal
*
*                       data    I/O
*       name            type    type    function
*       -------------------------------------------------------------------
*       cur_lsf         fxpt_16 i/o     Current frame's LSFs
*       prev_lsf        fxpt_16  i      Previous frame's LSFs
*
**************************************************************************/
void SynthCheckForMonotonicity(
fxpt_16 cur_lsf[ORDER],
fxpt_16 prev_lsf[ORDER])
{
        int     i;
        int     nonmono;

        /*  Try to fix any nonmonotonic LSFs by repeating pair  */
        for (i=1; i<ORDER; i++) {
                if (cur_lsf[i] <= cur_lsf[i - 1]) {
                        CELP_PRINTF(("Synthesis Interpolation: try to fix any nonmonotonic LSFs\n"));
                        cur_lsf[i] = prev_lsf[i];
                        cur_lsf[i - 1] = prev_lsf[i - 1];
                }
        }

        /*  Check fixed LSPs (also check for pairs too close?)  */
        nonmono = FALSE;
        for (i=1; i<ORDER; i++) {
                if (cur_lsf[i] <= cur_lsf[i - 1])
                        nonmono = TRUE;
        }

        /*  If fix fails, repeat entire LSP vector              */
        if (nonmono) {
                CELP_PRINTF(("Synthesis Interpolation: repeat entire LSP vector\n"));
                for (i=0; i<ORDER; i++) {
                        cur_lsf[i] = prev_lsf[i];
                }
        }
}
